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ABSTRACT 


An Autonomous Underwater Vehicle (AUV) can combine a 
Global Positioning System (GPS) recelver with an Inertial 
Navigation System (INS) to navigate with a specified 
accuracy level. The AUV would be required to surface 
periodically to obtain a GPS fix. A computer simulation has 
been developed using an AUV model and an INS error model to 
generate noisy measurements. A Kalman filter is used to 
estimate the simulated INS errors. Several runs were 
executed to compare combinations of equipment with different 


levels of accuracy. 


iii 


TABLE OF CONTENTS 


I.. INTRODUCTION . . . . +. . 


II. GLOBAL POSITIONING SYSTEM 


A. LEVELS OF ACCURACY . 


B. POSITION DETERMINATION 


C. REPRESENTATIVE GPS DATA 


III. INERTIAL NAVIGATION SYSTEMS 


A.  GIMBALLED SYSTEMS . 


B. STRAPDOWN SYSTEMS . 


C. GYROSCOPE PERFORMANCE CHARACTERISTICS 


D. OPTICAL GYROSCOPES 


1. Advantages of Optical Gyroscopes . 


2. Theory of Operation: 


3. Ring Laser Gyros 
IV. INS ERROR ESTIMATTONET: 


A. INS ERROR MODEL .. 


B. KALMAN FILTER EQUATIONS 


ve COMPUTER SIMULATION .. 


VT: RESULTS AA AS 


VII. CONCLUSIONS AND RECOMMENDATIONS 


APPENDIX A. 
APPENDIX B.: 
LISI OF REFERENCES SEER 
BIBLIOGRAPHY 1" 


INITIAL DISTRIBUTION LIST . . 


lV 


GPS DATA PROCESSING SOFTWARE 


COMPUTER SIMULATION SOFTWARE 


Sagnac Effect 


10 


13 


16 


16 


18 


24 


26 


26 


28 


31 


35 


35 


37 


41 


48 


70 


72 


74 


88 


90 


91 


LIST OF TABLES 


Table 2.1: ACCURACY LEVELS OF THE STANDARD POSITIONING 
SERVICE (SPS) AND THE PRECISE POSITIONING 
SERVICE (PBS - (o su o us e oa e sa o e e os 

Table 3.1: FAMILY OF GYROSCOPE APPLICATIONS ..... 


Table 4.1: INITIAL VARIANCES IN THE ERROR COVARIANCE 
MAREK - s-o € DW. . . e. o Gi” 


mooie 5.1: STATE VARIABLE VARIANCES ~~... .. . . « 


Table 5.2: ACCURACY LEVELS DURING THE COMPUTER 
AO DNS Pe + * * 


Figure 


Figure 


Figure 


Figure 


Figure 


Figure 


Figure 


Figure 


Figure 


Figure 


Figure 
Figure 
Figure 
Figure 
Figure 
Figure 
Figure 
Figure 
Figure 


Figure 


LIST OF FIGURES 


Open-loop alding of Inertial Navigation 
System (INS) measurements using Global 
Positioning System (GPS) data : 


Closed-loop aiding of INS measurements 
using GPS data. . . . . 


Integrated GPS/INS... . MM. MM 


Horizontal errors in GPS position 
measurements. . . . . . . . 


Vertical errors in GPS position 
measurements. . . . . . +. +. . . 


Errors in GPS velocity measurements . . . 


Reference frames used for inertial 
navigation systems. . . . . 


Local-level frame mechanization of INS 
measurement processing. . . . . 


Earth-fixed frame mechanization of an INS . 
Errors in an INS with a gyro drift of 0.01 
deg/hr and an accelerometer bias of 0.0001 
m/s e e e 9 2 MOM D. 
Illustration of the Sagnac effect 

Block diagram of computer simulation. . 
Block diagram of INS error model. . 

AUV depth during initial surface interval 
AUV depth during first five dives . . . . . 
attitude error. 


First-run, surface-interval 


First-run, surface-interval position error. 


First-run, surface-interval velocity error. 


First-run, surface-interval gyro drift. 
First-run, surface-interval accelerometer 
bias“: e e o . . e. o . o o e e e e e e 0 


vi 


14 


15 


15 


LZ 


19 


19 


24 


29 


41 


43 


48 


48 


49 


49 


50 


50 


50 


Figure 
Figure 
Figure 
Figure 
Figure 
Figure 
Figure 
Figure 
Figure 


Figure 
Figure 
Figure 
Figure 


Figure 


Figure 


Figure 
Figure 6.24: 
Figure 
Figure 
Figure 
Figure 
Figure 
Figure 


Figure 


First-run, 
First-run, 
First-run, 
First-run, 


First-run, 


dive-series 
dive-series 
dive-series 
dive-series 


dive-series 


attitude error. 
position error. 
velocity error. . . 
GLO Arift: A > e e 


accelerometer bias. 


First-run 
First-run 
First-run 
First-run 


First-run 


attitude error estimate error. 
position error estimate error. 
velocity error estimate error. 


gyro drift estimate error. . . 


accelerometer bias 


estimate 


rror... >» 


Second-run, 
error. . . 


Second-run, 
error B o e 


Second-run, 
eI OI. 5 


Second-run, 


Second-run, 
bias . . . 


Second-run, 
Second-run, 
Second-run, 
Second-run, 


Second-run, 


surface-interval 


surface-interval 


. o e o e o o o e 


surface-interval 


surface-interval 


surface-interval 


attitude 


position 


velocity 


gyro drift 


accelerometer 


dive-series 
dive-series 
dive-series 
dive-series 


dive-series 


attitude error . . 
position error . . 
velocity error . . 
gyro driftme: . 


accelerometer bias 


Second-run attitude error estimate error . 


Second-run position error estimate error . 


Second-run velocity error estimate error . 


Second-run gyro drift estimate error... 


vil 


51 


51 


51L 


52 


52 


52 


53 


33 


93 


54 


54 


54 


55 


55 


55 


56 


56 


56 


57 


57 


57 


58 


58 


58 


Figure 


Figure 
Figure 
Figure 
Figure 


Figure 


Figure 
Figure 
Figure 
Figure 
Figure 
Figure 
Figure 
Figure 
Figure 


Figure 


Figure 


Figure 


Figure 


Figure 


Figure 


Figure 


Figure 


Second-run 
error. > . 


Third-run, 
Third-run, 
Third-run, 
Third-run, 


Third-run, 
bias . ¿E 


Third-run, 
Third-run, 
Third-run, 
Third-run, 
Third-run, 
Third-run 
Third-run 
Third-run 
Third-run 


Third-run 


accelerometer bias estimate 


surface-interval 
surface-interval 
surface-interval 
surface-interval 


surface-interval 


o o . e o + o 


attitude error 
position error 
velocity error 
gyro drift TR; 


accelerometer 


dive-serles attitude error. . 


dive-series position error. . 
dive-series velocity error. . 
dive-series gyro drift. ... 
dive-series 
attitude error estimate error. 
position error estimate error. 


velocity error estimate error. 


gyro drift estimate error. 


accelerometer bias. 


accelerometer bias 


error. . . 


Fourth-run, 
error. . . 


Fourth-run, 
errors e 


Fourth-run, 
error: Sos 


Fourth-run, 


Fourth-run, 
bias . . . 


Fourth-run, 


Fourth-run, 


o o o o o e e o o 


surface-interval 


e. o . LJ + . . e o 


surface-interval 


e e e . + e o o o 


surface-interval 


o o o o o o o e e 


surface-interval 


surface-interval 


o o o + € + e o o 


estimate 


attitude 


position 


velocity 


o o e e e o o 


gyro drift. . 


accelerometer 


dive-series attitude error 
dive-series position error 


viii 


+ 


59 


DS 


59 


60 


60 


60 


61 


61 


61 


62 


62 


62 


63 


63 


63 


64 


64 


64 


65 


65 


65 


66 


66 


Figure 
Figure 
Figure 
Figure 
Figure 
Figure 
Figure 


Figure 


6159: 


6.60: 


Fourth-run, 
Fourth-run, 
Fourth-run, 
Fourth-run 
Fourth-run 
Fourth-run 
Fourth-run 


Fourth-run 
error. i . 


dive-series velocity error . . 
dive-series gyro drift... 

dive-series accelerometer bias 
attitude error estimate error 

position error estimate error . 
velocity error estimate error . 
gyro drift estimate error . .. 


accelerometer bias estimate 


ix 


66 


67 


67 


67 


68 


68 


68 


69 


TABLE OF SYMBOLS 


PEU position 

V, Vor velocity 

dp ~ INS position error 
dv ~ INS velocity error 

€ ^ INS attitude error 

ep - GPS position error 
ev - GPS velocity error 

~ estimate of e 

~ estimate error 

~ standard deviation 

~ pseudorange measurement 
~ noiseless pseudorange 
~ measurement noise 


2 time-correlated errors 


c X» 4 €70 0 00 


X; Y; Z; ~ position of Satel ccu 
x= [x, y, z]* - user's position 
At - receiver's clock offset 
C ~ speed of light 
X 7 [x, Yor Zol” ~ approximate user's position 
b ~ body frame 
e ~ earth-fixed frame 
n~ local-level frame 
1 ~ inertial frame 
Y - latitude 
A ~ longitude 
h ~ altitude 
y ~ normal gravity 
Í? - specific force in b-frame coordinates 


R”, ~ transformation matrix from b- to n-frame 


R ~ change in the R matrix 


Y - acceleration 


QP., - angular rotation rate of the b-frame with respect to 


the i-frame, given in b-frame coordinates 


Q? ~ skew-symmetric matrix of o’, 
[2, 3, K]7 - unit vector 

O, - earth’s rotation rate 

(Pap), = roll rate 

(05,5), - pitch rate 

(Wop) ~ yaw rate 

k ~ sample number 

g 


~ gravity vector 


p(t) ~ position as a function of time 
v(t) ~ velocity as a function of time 
b, a ~ accelerometer bias 
Mg ~ gyro drift 
R ~ radius of FOG coil 
~ number of turns in FOG coil 
~ area of FOG coil 

~ earth's radius 
~ Schuler frequency 

transit time 

FOG rotation rate 
~ optical path length difference 
optical frequency 

optical path length 

optical wavelengtn 

net number of accumulated fringe counts in a Sagnac 
interferometer 
- net angle turned 
^ fringe pattern intensity 

- average intensity 


w ~ angular beat frequency 

€ ~ fringe spacing 

~ distance along fringe pattern 

~ arbitrary phase angle 

~ state vector 

~ white noise 

[F] ~ dynamics matrix 

[I] ~ identity matrix 

[0] ~ null matrix 

F? - skew-symmetric matrix of f? 

C ^ Gauss-Markov constant for drift 
D - Gauss-Markov constant for bias 
G ~ system noise matrix 

P(K) - discrete version of G 

(k) ~ transition matrix 

z(k) ~ measurements 

t, sample time 

H(k) - observation matrix 

v(k) ~ measurement noise 

Q(k) ~ system noise covariance matrix 
R(k) ~ measurement noise covariance matrix 
K(k) ~ Kalman galn matrix 

P(k) ~ error covariance matrix 
P"(k) ~ initial estimate of P(k) 
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I. INTRODUCTION 


An Autonomous Underwater Vehicle (AUV) is a submersible 
designed to operate independently of real-time human 
control. In order to carry on planned missions, such a 
vehicle requires a navigation system to determine its 
location at any time. For covert and long range operations, 
passive navigation is preferable, and has to be designed to 
provide sufficient accuracy during the entire mission. 

A passive and accurate navigation system can be 
developed by combining an Inertial Navigation System (INS) 
with a Global Positioning System (GPS) receiver ina 
complementary fashion. Since the errors in any INS grow 
with time, they can be corrected by programming the AUV to 
surface periodically to get a GPS fix. 

The GPS uses satellites to determine position and 
velocity. A measurement of the time-of-arrival of the GPS 
Signal at the receiver is combined with knowledge of the 
satellite’s position to estimate the range to the satellite 
and the user’s clock error. This measurement is referred to 
as the "pseudorange". Another measurement, called the 
"delta range", is used to determine the user’s velocity from 
the Doppler shift of the GPS carrier frequency. The 


accuracy of velocity determined by GPS is based on knowledge 


of the orbital parameters of the satellites and the short 
carrier wavelength, which is approximately 19 cm (Ref. 1]. 

The accuracy of GPS position data depends on several 
factors, which are discussed in Chapter 2. There are 
differences, in terms of accuracy, between military and 
civilian applications, authorized and non-authorized users, 
respectively. An independent authorized user can determine 
position with a two-dimensional (2-D), horizontal accuracy 
of better than 17.8 meters (Ref. 2]. Non-authorized users 
can obtain commercially available equipment that provides 
independent position estimates with a 2-D accuracy on the 
order of 100 meters. If a reference station is available, 
then differential corrections can be used to provide 
accuracy of better than five meters for any user. 

An INS uses accelerometers to measure the forces acting 
on a vehicle, and gyroscopes ("gyros") to determine the 
direction of those forces. Vehicle accelerations can be 
derived from the measured forces. By integrating the 
accelerations we can compute velocities, and with a second 
integration we obtain the position relative to the initial 
conditions. An INS also outputs the orientation of the 
vehicle, because this attitude information is required when 
determining the force directions. 

The accuracy of INS data depends on the quality of the 
equipment. The trade-offs for better accuracy typically 


involve size, weight, and cost. 


An INS can be used to measure a vehicle’s trajectory. 
One problem with using an INS is the growth of errors over 
time due to the integration of acceleration. This drawback 
can be overcome by combining the INS with independent 
position and velocity measurements, in order to estimate and 
correct the INS errors. 

This thesis addresses the use of a combination of INS 
and GPS equipment for the navigation of an AUV. Since GPS 
signals cannot be received underwater, GPS position and 
velocity measurements are obtained by requiring the vehicle 
to surface periodically. The integration of INS and GPS 
data is implemented using. Kalman filtering techniques. 

The Kalman filter approach described in this research is 
based on statistical models of the errors inherent to INS 
and GPS measurements. In particular, the INS gives 
measurements of position, velocity, and angular orientation 
of the vehicle in different coordinate frames (inertial and 
body fixed), while the GPS yields measurements of position 
and velocity in an earth-fixed frame. The two systems are 
combined as shown in Figure 1.1, where we can see that the 
Kalman filter attempts to determine an optimal estimate of 
the errors. The estimates are consequently used to correct 
one of the measurement sets (the INS, for example). This 
configuration is an open-loop implementation. The task of 
determining error models is of fundamental importance in 


this problem. 


indicated while the vehicle is near the surface. 





| p « $p P + Sp 


V+ Sy V + Sv 
attitude * , atto 







INS 






Ete 4 


| V + ev 

GPS AZ) 
ETEN cocă 

AND compass OEY S 


NOTATION: & = INS position error 


ep = GPS position error | 
IN 













ER 


- - error 





| òp = INS position error estimate 
| ~ A 

sp = Sp - = estimate error 

V = velocity 


Figure 1.1: Open-loop aiding of Inertial Navigation 
System (INS) measurements using Global Positioning System 
(GPS) data. (After Ref. 3:p. 266) 


The following approach can be used to determine the 
attitude errors. To obtain the error in pitch angle, a 
sample is taken when the depth rate sensor indicates zero 
vertical velocity. Assuming the AUV is in transition from a 
climb to a dive at this instant, the pitch angle is zero and 
the angle indicated by the INS is the error. The roll angle 


error is obtained by averaging samples of the roll angle 


measurement is based on the assumption that the vehicle is 
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This error 


inherently stable and the roll angle will average zero 
during this interval. The heading error can be obtained by 
comparing a compass reading to the INS indication. 

Figure 1.2 illustrates the alternative closed-loop 
implementation, which feeds the error estimates back into 
the INS. The INS then uses the error estimates to compute 
corrected position, velocity, and attitude measurements. 
This leads to a filter that linearizes about an estimated 
trajectory that is updated with each aiding measurement. 
This approach is called extended Kalman filtering and is the 
preferred method for long duration missions on the order of 


weeks or more [Ref. 3:pp. 356-379]. 
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Figure 1.2: Closed-loop aiding of INS measurements using 
GPS data. (After Ref. 3:p. 369) 


Both versions of GPS-aiding of an INS use position and 
velocity estimates from a Kalman filter within the GPS unit. 
The integration of GPS and INS measurements can be 
accomplished with a single Kalman filter, as illustrated in 


Figure 1.3 [Ref. 4]. 
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Figure 1.3: Integrated GPS/INS. (After Ref. 4) 


This thesis focuses on using the open-loop approach to 
aiding an INS with GPS. In this case the Kalman filter 
linearizes about the reference trajectory provided by the 
INS. Chapter II discusses GPS in terms of the available 
levels of accuracy and the procedure for determining 
position from the raw measurements. Some pre ANE GPS 
data is included in chapter II to illustrate the typical 


errors inherent to the systen. 


In chapter III, the various types of INS’s are described 
and background information is provided to clarify the 
terminology. The INS error model is developed in chapter 
IV, along with the Kalman filter equations. 

The computer simulation is described in chapter V, and 
the results of several runs are presented in chapter VI. 
Finally, chapter VII provides conclusions and 
recommendations for further work in this area. Software 
listings are included in the appendices, along with a 
bibliography, to assist with related research. The software 
has not been verified beyond the results documented here and 


further application is the responsibility of the user. 


II. GLOBAL POSITIONING SYSTEM 


A. LEVELS OF ACCURACY 

The GPS is a Department of Defense (DOD) satellite 
navigation system. The primary purpose of the GPS is to 
support military navigation requirements. Two levels of 
accuracy are being provided by the GPS in accordance with 
DOD policy. Authorized users, mostly military, are allowed 
to use the high-accuracy capabilities of the system. Some 
applications outside the military have been granted 
permission to use these capabilities. All other users are 
referred to as non-authorized users, but they have unlimited 
access to the low-accuracy capabilities. 

Authorized users are provided with a high-accuracy 
navigation system that is totally passive, i.e., it does not 
require radiating signals from the user. The advantages of 
non-radiating user equipment include less power required, 
smaller size units, and the ability to operate covertly. 
These advantages are all due to the absence of a 
transmitter. 

The use of the GPS by non-authorized users is being 
permitted at reduced accuracy levels. The techniques used 
for degrading the accuracy are referred to as Selective 
Availability (SA). The actual accuracy levels provided can 


be adjusted by the DOD in accordance with policy. In fact, 


SA can be turned off entirely. The current policy (April 
1992), states that two times the standard deviation of 
horizontal position will be less than or equal to 100 meters 
(Ref. 2). The Greek letter "sigma" is commonly used to 
denote the standard deviation, so this measure of accuracy 
is referred to as the "two sigma" (2c) level. The 
horizontal position is the standard 2-D solution, because it 
is the usual type of position required by ships at sea. 

The authorized users will have access to the Precise 
Positioning Service (PPS), whereas non-authorized users will 
be limited to the Standard Positioning Service (SPS). The 
accuracy levels associated with SPS and PPS are compared in 
Table 2.1. 

Table 2.1: ACCURACY LEVELS OF THE STANDARD POSITIONING 


SERVICE (SPS) AND THE PRECISE POSITIONING SERVICE (PPS) 
(After Ref. 2) 


HORIZONTAL POSITION (20 


) 
VERTICAL POSITION (20) xi 


RECEIVER CLOCK SYNCHRONIZATION (lo) 





Another difference between SPS and PPS, in addition to 
SA, is the signal structure. For authorized users, a 
Precision code (P-code) is broadcast on two frequencies, L, 
and L,. A Coarse/Acquisition code (C/A code ) on L, provides 
a coarse navigation capability to SPS users, as well as a 


quick acquisition capability for PPS users. The PPS’s use 


of two L-band frequencies, 1575.42 Mhz (L,) and 1227.6 Mhz 
(L,), provides a means for correcting for ionospheric delays 
RECO 

In addition to navigation, other applications are being 
found for the GPS, including geodesy and synchronization. 
Geodetic surveys usually require collecting data at a 
stationary site for extended periods of time. Time 
synchronization systems involve placing a GPS antenna at a 
surveyed location and solving for the receiver clock error. 

Another application of receiving GPS signals at a 
surveyed site is for using differential corrections to 
improve the accuracy of a mobile unit's position solution. 
Using differential techniques, the accuracy available for 
all users can be better than five meters. 

The use of interferometric techniques have proven to 
further improve the accuracy available, but continuous 
tracking of the signal carriers is required. Continuous 
tracking is difficult for maneuvering aircraft and is 
unfeasible for submersibles. 

For both SPS and PPS, the accuracy of the GPS data is also 
dependent on the relative positions of the satellites and 
the user. The relationship of satellite geometry to 


position accuracy is explained in the next section. 


B. POSITION DETERMINATION 


This section develops the equations used to determine 
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position based on GPS measurements. For more detailed 
information, the reader is referred to [Ref. 1] and [Ref. 
3:pp. 409-423]. The notation used in the following 
development is consistent with (Ref. 3:pp. 409-423]. 

The measurement used for the determination of a 
receiver’s position is the GPS signal’s time-of-arrival. 
This observable is referred to as the pseudorange, because 
it includes a bias due to the receiver’s clock offset. The 
pseudorange measurement, p, includes the noiseless 


pseudorange, W, along with the measurement noise, v and 


p! 
time-correlated errors, p, [Ref. 3:p.412]. 


The GPS satellites broadcast information that includes 
orbital parameters, referred to as ephemerides. These 
ephemerides are used to calculate the satellite's position, 
(X, Y; Z)" for satellite i. Four measurements are required 
to solve for the user's position, x - [x, y, z]', and the 
receiver's clock offset, At. From (Ref. 3:p. 410] 

d, 7 405207 * (197 * ZI + cat 
We VORT ETT ZAJ kc AL "T 


RN uU M E (2,52) + CAL 
= loko)” 1 MS + ORALE, 


where c is the speed of light. If the altitude is known, 
e.g., a vessel is on the sea’s surface, than only three 
pseudorange measurements are required. 


Most GPS receivers use Kalman filtering, which requires 


dr 


the linearization of Equation 2.1. 
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These partial derivatives 


are the direction cosines from the satellite to the 


approximated user’s position. 


Subtracting the predicted pseudorange, $, from the 


measured pseudorange gives the noiseless measurement 


equation: 
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where (Ax, Ay, Az]' = x-x, [Ref. 3:p. 422]. 


These measurements are based on the direction cosines. 


Therefore, the accuracy of the position estimates is 


dependent on the satellite geometry. 
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C. REPRESENTATIVE GPS DATA 

Data was collected with a GPS receiver to determine the 
statistics of position and velocity measurements. A C/A 
code receiver was used and therefore a comparison can be 
made with the published SPS position accuracies. 

The specific GPS receiver used was a Magnavox model MX 
4200 purchased by the Naval Postgraduate School. The data 
was stored on a laptop computer and software programs were 
written in FORTRAN to unpack the position and velocity 
measurements. Source code listings of these programs are 
provided in appendix A. A Microsoft FORTRAN compiler was 
used and these programs were run on IBM-compatible personal 
computers (XT and AT). 

The GPS antenna was located on a surveyed antenna mount 
at Pt. Mugu, California. The surveyed position was 
subtracted from the measurements to obtain GPS errors. To 
convert from degrees of latitude and longitude to meters, 
correction factors were obtained from (Ref. 5). 

The horizontal position errors are shown in Figure 2.1, 
and Figure 2.2 shows the vertical position errors. As seen 
in Figure 2.2, the vertical position error is constant for a 
period between sample 500 and sample 1000, this is due to 
the GPS receiver switching to an altitude-hold mode where 
only three satellites are used to determine the position. 
Figure 2.3 shows the velocity errors, which were converted 
from knots to meters per second. 
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The standard deviation of the horizontal position errors 
was computed to be 43.1 meters, which is close to the 
expected value of 50 meters based on the DOD policy. For 
the vertical position errors, the standard deviation was 
computed to be 81.7 meters, which again is close to the 
expected value of 78 meters. The velocity errors had a 


standard deviation of 0.9 meters per second. 
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Figure 2.1: Horizontal errors in GPS position measurements. 
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Figure 2.2: Vertical errors in GPS position measurements. 
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Figure 2.3: Errors in GPS velocity measurements. 
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III. INERTIAL NAVIGATION SYSTEMS 


A. GIMBALLED SYSTEMS 

An INS measures accelerations and rotations in order to 
compute position, velocity, and orientation of a platform, 
usually a moving vehicle. A typical INS uses a triad of 
accelerometers, mounted on orthogonal axes, to measure the 
specific forces, f, experienced by a vehicle. Early systems 
mounted these accelerometers on a platform within an 
arrangement of gimbals. Sets of gyroscopes and servos were 
used to keep the platform stable within a reference 
coordinate system. Any rotation of the platform was sensed 
by the gyros and the servos would rotate the gimbal axes as 
required to counter the sensed rotation. These gimballed 
systems are still used and provide satisfactory accuracy at 
the expense of size, weight, and the low reliability 
associated with mechanical systems. 

A typical reference coordinate frame used for gimballed 
systems is the local-level frame ("n-frame") as illustrated 
in Figure 3.1. The coordinate axes are aligned with the 
local north, east, and down directions. The advantage of 
using this system is that the force of gravity is registered 
on only one of the accelerometers. This means that the 
forces sensed by the horizontal accelerometers are directly 


related to the vehicle accelerations in the familiar 
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directions of latitude, $, and longitude, A. These 
accelerations can be integrated once to give velocities and 


again to provide changes in position from some starting 


location. 
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Figure 3.1: Reference frames used for inertial navigation 
systems. (After Ref. 6) 
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B. STRAPDOWN SYSTEMS 

With the development of the digital computer, an 
alternative configuration has become practical. By hard- 
mounting the accelerometers to the vehicle and using 
gyroscopes to measure rotations, w, a large part of the 
mechanical complexity can be reduced. The computer is 
required for keeping track of the orientation of the 
instrumentation package, so that the directions of the 
measured accelerations are known. Transformation matrices 
are used to convert vectors from one reference frame to 
another. For example, R' transforms a vector in body frame 
("b-frame") coordinates to n-frame coordinates. 

With these strapdown systems, the measurements are made 
in the b-frame, defined by the forward, right, and down 
directions (Ref. 7). Thus, the rotation measurements give 
roll, pitch, and yaw directly. The problem is that, in 
addition to vehicle motion, each accelerometer measures 
components of other forces due to gravity and Coriolis, 
caused by the earth's rotation. These forces have to be 
estimated and subtracted, in order to obtain the 
accelerations due to vehicle motion. 

Figure 3.2 illustrates the process of converting the 
inertial measurements into attitude, position (@, A, h), and 
velocity (v). The results are in the local-level frame if 
the proper transformations from the b-frame to the n-frame 


are included in the processing. 
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Figure 3.2: Local-level frame mechanization of INS 
measurement processing. (From Ref. 8) 


An alternative INS mechanization, illustrated in Figure 
3.3, involves transformations from the b-frame to the earth- 


fixed frame ("e-frame"), labeled x, y, and z in Figure 3.1. 
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Figure 3.3: Earth-fixed frame mechanization of an INS. 
(From Ref. 8) 
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It has been shown in (Ref. 8] that the processing of 
measurements using the e-frame can be accomplished faster 
than when using the n-frame. When using the e-frame, less 
computing time is required for the mechanization equations 
and the Kalman filter. More time is required for the 
computation of normal gravity, but the net result is faster 
processing with the e-frame algorithm. 

In this section we develop the equations for position 
and velocity estimates in the e-frame in terms of the forces 
acting on the vehicle. The equations are based on the 
developments in (Ref. 8) and (Ref. 9]. The reference frames 
used are the body frame (b), the earth-fixed frame (e), the 
local-level frame (n), and the inertial frame (1). Vectors 
and matrices are annotated with subscripts and superscripts 
depending on which frame or frames they reference. For 
instance, position and velocity in e-frame coordinates are 


denoted rf and v? respectively. Similarly, Q^, is the skew- 
symmetric matrix of 9?,, which is the angular velocity 


vector of the body frame with respect to the e-frame, given 
in b-frame coordinates. 

The skew-symmetric matrices are used to execute the 
cross-product operation. For instance, the Coriolis 
acceleration is two times the earth's rotation rate, 0,, 


crossed with the vehicle’s velocity, 
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Mq Mc vel 20.v9* 7 .*20.v9.J, , (3.1) 
where the ^ denotes a unit vector. The skew-symmetric 
matrix serves to simplify the notation 

0 -20,0]V^4 |-20,v?, 
2 re lago Wee = 20, 0 0 ve, = 20 vo, i (352) 


0 0 0 ve. 0 


where Q“,, is the skew-symmetric matrix of the angular 
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VEkocCity of the earth's rotation, Qf,- 


The INS measurements are the specific force vector, fi, 


and the angular rotation rate of the body with respect to 


the inertial frame, w” Subtracting the earth’s angular 


ib ° 


velocity vector, a? from a, gives 


lo! 


- w?. p (33) 


1b 1e 


b = b 
p? o, =”, 


which is used to build the skew-symmetric matrix 


O > - (0? eb) 3 (Gu); (3.4) 
D 2 = (WP ap) 3 0 = (o5 ,,) q / 
" (0? ¿p) 2 («P s) T 0 
where (9P,), is the roll rate, (o,), is the pitch rate, 


and (02), is the yaw rate. 
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Accelerations are found by transforming the force 
measurements and subtracting the acceleration due to gravity 
and the Coriolis effect. The change in the transformation 


matrix from body to earth coordinates is given by 


Rs = RS OF NC . (3.5) 


The earth's angular velocity vector in body coordinates, 


b 


Q^. , is obtained from Q: according se 


ie 


w?. QE , (3.6) 


1e 


where the transformation matrix R^, is the inverse of R*,. 


Transformation matrices involved are orthogonal, and 


therefore their inverses are equal to their transposes: 


RP = [RETA = PREM (3.7) 
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We can relate different transformation matrices as (Ref. 6]: 


RO RERO RROME (3.8) 


where the transformation matrix, R”,, is obtained from 


initialization and the time history of body rotations. 
Using the geometry of Figure 3.1, and unit vectors 


along the e-frame and n-frame coordinate axes: 
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hi” d -sin cosà i, - sin$ĝsinA j, + cosok, 
D = Ku p - SERNA ra + cosà J, , (3.9) 
k k,|  |-cosgcosi î, - cospsini 3, - sindk, 


where 9 denotes latitude and A denotes longitude. 


Therefore, 
-sindĝcosA -sinĝ sinA cos 
R^, = -sinA cosA 0 (3.10) 
-cos cos] -cosbsini -sind 
and 
-sinbcosA -sinA -cosĝcosA 
R?*,- [R^,]7T = |-sin$sinA cosA -cosĝsinA| . (3.11) 


cos 0 -sin 
After initialization, R*,, is obtained by integrating 
k 
p Ck NERIS OD MEM XS oa (ANE). (3.12) 
0 
where k is the number of the current sample and At is the 


sampling interval. 


In summary, the mechanization equation is given by 


qs vi 

SZENES KEK DS vd gre, (313) 
; b 

R^, Rp 9” eb 


where g? is the gravity vector. 
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C. GYROSCOPE PERFORMANCE CHARACTERISTICS 

The performance characteristics used for comparing gyros 
include gyro drift, scale factor error, and random walk 
noise. Other parameters of importance are the size, weight, 
cost, and dynamic range. The dynamic range is the range of 
input rotation rates that can be measured correctly. 

The gyro drift is also referred to as bias error or bias 
stability and is given in units of angle per unit of time 
(e.g., deg/sec). It is also common to describe a gyro's 
performance in terms of nautical miles (nmi) per hour. This 
measure is based on how the gyro would perform in an INS. 
Figure 3.4 shows how the position error grows with time for 
a given gyro drift. This plot shows that for a gyro bias of 
0.01 deg/hr, in combination with an accelerometer with a 
bias of 0.0001 m/s’, the performance is on the order of 1 


nmi/hr. 


Position error va. time 





Figure 3.4: Errors in an INS with a gyro drift of 0.01 
deg/hr and an accelerometer bias of 0.0001 m/s”. 
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Figure 3.4 is based on a model given in (Ref. 10]. This 
model is valid for mission times of less than three hours. 
For missions of this duration, the Schuler oscillation is 
important, but the 24 hour effects can be neglected. The 


model is given by 





p(t) = = (1-cos@,t) + gk, (t-—-sinw,t) 
Os a s i (3.14) 
v(t) = y Sino,t * gR,(1-coso,t) 


S 


where p(t) is the position (given as a function of time), 
v(t) is the velocity as a function of time, a is the 
accelerometer bias, g is the gyro drift, R. is the earth's 


radius, and w, is the frequency of the Schuler oscillation. 


The scale factor is the resolution of the system. The 
scale factor error is given in parts per million (ppm) or 
percent (%). Gyroscope noise is primarily due to random 
walk and is given in units of degrees per square root hour. 

System requirements are different depending on the 
application. A submarine for launching ballistic missiles 
can carry a large instrument package and requires high 
accuracy navigation. Since submarines remain submerged for 
extended periods of time, they cannot tolerate large error 
growth. Fortunately, these characteristics are compatible, 
i.e., for higher accuracy and smaller error growth, a large 


volume is required. 
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For commercial alrcraft a medium-grade system is 
sufficient and usually preferred, due to the reduced cost. 
Such a system, with medium-level accuracy, is commonly 
referred to as an Attitude Heading Reference System (AHRS). 
For a tactical missile with a short flight time, a low 
accuracy instrument is acceptable. 

Very high accuracy gyroscopes are used for scientific 
experiments related to geophysics and relativity. 
Geodesists and surveyors routinely use INS's, including 
those with Ring Laser Gyros (RLG's). Table 3.1 summarizes 
the main performance characteristics and provides typical 


value ranges for gyros of different levels of accuracy. 


Table 3. l: FAMILY O OF _GYROSCOPE APPLICA APPLICATIONS TIONS (After Ref. 11 
TTA RMANCE LOW-GRADE MEDIUM-GRADE HIGH-GRADE 

ARAMETER (munitions) (missiles) (submarines) 
Bias 0.5 001 = 0.001 - 0.01 
| Stability a CE dis iie deg/hr 


Random Walk UR = UNUC "—- er OS UUN 
(deg/root hr) 


Scale Factor 0.1 - 5% 100 - 5000 < 10 
Linearity ppm ppm 


D. OPTICAL GYROSCOPES 








1. Advantages of Optical Gyroscopes 
Prior to the development of the laser, mechanical 
gyroscopes were used extensively. These mechanical gyros 


consist of a spinning mass along with a motor to provide the 
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torque required to keep the mass spinning. Great care is 
taken during manufacturing to produce a balanced mass and 
reduce friction, but there is always some residual imbalance 
and friction contributing to measurement error. These 
mechanical devices are sensitive to large accelerations 
(g's) and have larger errors in a high-dynamic environment. 

The idea of using interferometry to measure rotation 
rates dates back to before the turn of the century, and 
Sagnac demonstrated this capability in 1913 during ether 
experiments (Ref. 12]. The first experiments with a laser 
gyro were performed in 1962 (Ref. 12]. Since that time the 
Ring Laser Gyro (RLG) has been developed into a practical 
instrument and is operational on alrcraft ranging from the 
commercial Boelng 757/767 series (Ref. 13) to the military 
F-15 and UH-60 (Ref. 14). Research and development of an 
operational Fiber Optic Gyro (FOG) is ongoing and flight 
tests have been conducted (Ref. 14]. 

The major advantage of optical gyroscopes is the 
large reduction in the number of moving parts. In fact, 
FOG's are completely solid-state [Ref. 15]. This primary 
advantage provides several secondary benefits. With fewer 
moving parts, optical gyroscopes are easier to maintain, are 
more reliable, and are more rugged. In addition, for a 
given level of accuracy, optical gyros can be smaller and 
lighter, and have a lower total parts count. Components for 


FOG's are readily available due to the increased use of 
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fiber optics ln communication systems. All of these factors 
contribute to lower costs. 

Additional advantages are the large reductions in 
both warm-up time and sensitivity to high-dynamic maneuVers. 
According to [Ref. 15), FOG's are being developed for "smart 
munitions" which experience up to 20,000 g's when fired from 
a cannon. 

Optical gyros can be used to support navigation of 
platforms ranging from space vehicles to submarines. Indeed, 
this technology is currently replacing the traditional 
mechanical implementation. 

2. Theory of Operation: Sagnac Effect 

The Sagnac effect can be described [Ref. 16] in 
terms of the transit times of counter-propagating beams 
traveling in a circular optical cavity of radius R, as 
illustrated in Figure 3.5. The light enters the system from 
point A and the beam splitter diverts some of the light into 
the clockwise path and some of the light into the counter- 
clockwise path. If there is no rotation, then the beam 
splitter will not move and the two beams will recombine and 
return to point A. In this case, the transit time t is the 
Same in both directions and is given by 


r= 2R E (3.15) 
C 
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SAGNAC EFFECT 


R 
A = 
f 
beam splitter AX 





Figure 3.5: Illustration of the Sagnac effect. 


When the sensor rotates at a rate of O rad/sec, the 
beam splitter moves a distance X from point A to point B, 


where 


X=RQT . (3.16) 


For the beam traveling in the direction of the rotation, the 

transit time is 

- 2RR+X _ 2nR+RQT SW (3.17) 
E C 

For the oppositely directed beam, the transit time is 


=. = 2NR- RAI (3.18) 
= 
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Factoring out the transit time and combining the two 
equations: 


5.152) 2 3a 


The difference in transit times is 


2RR _ 2RR 











AT =T,-T. = 3.20 
e c-RQ. c+RQ ( ) 
Placing the two terms over a common denominator gives 
2 
e NS (EN 
c?-R?Q? 
Since c?»R?(Q?: 
2 
ncm RA (3122) 
C 
The optical path length difference is 
2 
AL = câz = AERO I (325) 
E 
For a circle of area A = nR? 
AL = 442 (3.24) 


It can be shown that Equation 3.24 is valid for any 
geometric shape of area A. This equation shows how a 
rotation causes a change in optical path length. It also 


shows that increasing the area of the optical path will 
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increase the optical path length difference. For rotation 
rates of interest in most applications, an impractical area 
would be required if no alternatives were available. 
Fortunately, by using N turns of a fiber optic cable, the 
area is multiplied by N and practical systems can be built. 

The FOG is a passive interferometer and using a coil 
of N turns of cable provides the required sensitivity. A 
RLG is an active interferometer and its ability to provide 
the necessary sensitivity is described in the next section. 

3. Ring Laser Gyros 

An active interferometer improves the sensitivity 
because the laser frequency depends on the cavity length 
(Ref. 16). With a gain medium within the optical cavity, 
the condition for oscillation is that an integral number of 
wavelengths fits within the cavity length. In a ring laser 
the two oppositely traveling waves can have different 
amplitudes and frequencies. A small change in the optical 
path length leads to a small change in frequency given by 


Av - AL 


- : (3.25) 


Since the optical frequency, v, is on the order of 
10^ Hz, small differences in length lead to changes in 
frequency that are large enough to be measured. From the 


Sagnac effect, 


AL = 448 | NSG) 


and the definition of optical frequency (v=c/A), the 


frequency difference is given by 








= 
Av ~ Av ` AL- d (3429) 
y (3) L L 
A 
Therefore, the beat frequency is: 
4AQ | c 4AQ 
Av = =] = —— . 3.28 
TANE). En Ge 
The net number of accumulated counts is found by 
integration: 
E t AAQ 4A f€ 4A0 
N= Avdt = == ~ado SIKLOJ RS A 329 
f. f. LA A i LA ( ) 


where ð is the net angle through which the gyro has turned. 
For example: given L = 39.6 cm, A = 75.45 cm, and a He-Ne 
laser with a wavelength of 0.633 um, setting N = 1 gives a 


resolution (scale factor) of 8... = 8.3 x 10“ rad = 1.7 


arcsec. This is equivalent to 7.6 x 10” counts per 
revolution or 2118 counts/deg. 

The read-out is obtained by using a prism behind one 
of the RLG’s mirrors. The intensity of the resulting fringe 
pattern is given by 


I = I,[l+cos(2ne/A+Aw trO] , E 


where Aw is the angular beat frequency, A/e is the fringe 
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spacing, x is the distance along the pattern, and $ is an 
arbitrary phase angle (Ref. 16]. The fringes can be counted 
by using detectors smaller than the fringe spacing. Two 
detectors, separated by a quarter fringe, can be used to 
sense the direction of rotation. 

For an ideal RLG, the output N is a linear function 
of the input 8. The relationship between the input and 
output is referred to as the characteristic curve. Any 
deviation from the ideal case is considered a scale-factor 
error. The scale factor determines the slope of the 
characteristic curve. A phenomenon, referred to as mode 
pulling, changes the slope of the characteristic curve. 
Mode pulling is due to changes in the index of refraction 
within the gain medium. 

Another problem in RLG'/s is the lock-in due to back- 
scattering from the mirrors. At low rotation rates, a RLG 
will not respond. The typical method of overcoming lock-in 
is to mechanically dither the RLG, or its mirrors, using 
piezo-electric transducers so that the RLG operates outside 
of the problem area. The characteristic curve can also be 
shifted up or down so that a non-zero reading is obtained 
without any rotation. This effect is called null shift. 
Anything besides a rotation that influences the oppositely 
directed beams in an unequal manner will contribute to the 
null-shift effect. Such phenomena are known as 


nonreciprocal effects. 


33 


In order to get three RLG's with the largest area 
possible for a given volume, the three optical cavities can 
be combined within a single block (Ref. 16]. With this 
configuration, each mirror is shared by two of the optical 


paths, thus reducing the parts count (Ref. 17]. 
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IV. INS ERROR ESTIMATION 


A. INS ERROR MODEL 

The following development uses the techniques in (Ref. 
8] and (Ref. 18) to derive the equations governing the INS 
error dynamics. This INS error model is used in the Kalman 
filter to estimate the INS errors. There are 15 states 
based on five vectors. The first three states are the e- 
frame components of the attitude error, £. The next six 
states are the e-frame components of the position error, 
óp, and the velocity error, Ay. The last two vectors are 
the gyro drift, d, and the accelerometer bias, b, given in 
b-frame coordinates. 

The state vector has been augmented with the drift and 
bias terms, because they are modeled as Gauss-Markov 
processes. This augmentation has been done so that the 
system noise, w is white. The gyro drift noise and the 


accelerometer noise will have standard deviations of 02, 
and 0,, respectively. The resulting state vector [Ref. 8] 


for the error signal can then be written as 


x= (2, Ap, Av, g, by (4.1) 
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and it is modeled as a standard state space equation 


X= [Fl] x + y, (4.2) 


where [F] is the dynamics matrix, determined on the basis of 
the system error equations. 


In particular, the misalignments are modeled as 


i = -09,,£+R,d . (4.3) 


The position errors are only dependent on the velocity 


errors: 


ŝp = [I] ŝv, (4.4) 


where [I] is the identity matrix. 

The velocity errors can be affected by the normal 
gravity error. However the height of the AUV at the time of 
the measurement will be known to be sea level. Therefore, 
according to (Ref. 18], this error source can be neglected. 
We are left with the effects of the attitude errors coupled 
into the force measurements, the velocity errors coupled 
into the Coriolis force calculations, and the accelerometer 


biases. This is expressed by the equation 


Ov--F*g-2 0" "D KD , (4.5) 


where F^ is the skew-symmetric matrix of the measured 


forces, f^, transformed into earth-fixed coordinates. 
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Modeling the gyro drifts and the accelerometer biases as 
first-order Gauss-Markov processes with (= 1 / (drift 


correlation time) and B= 1 / (bias correlation time): 


d=-(d + w , (4.6) 


and 


b=-Pb+w . (457) 


The resulting model for the system errors then yields a 


dynamics matrix 


-Qe, [o] to] Re, [0] 
[OJ TALOJ" [E] [0] [0] 
[F] =] -F° [0] -20°,, [0] R*, |. (4.8) 
[o] [o] or LO] 
n [0] S50] 107 [ols —B ET 


where (0) is a three-by-three null matrix. 


B. KALMAN FILTER EQUATIONS 

Kalman filtering techniques can be used to estimate the 
INS errors based on the model given by Equations 4.1, 4.2, 
and 4.8. For the given AUV scenario, Equation 4.2 can be 


rewritten as 


Me | eh GoW, (4.9) 


where G is the system noise matrix, and, from Equations 4.3 


through 4.7, 
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[O] [O] 


[O] [0] 
G = | [o] [O] (4.10) 
[I] o, [o] 
| [0] [1] 0)) 


Standard techniques, such as those described in (Ref. 
3), can be used to discretize Equation 4.9. The resulting 


discrete version is 


X(k+1) = $(k)x(k) + T(k)w([k) , (4.11) 


where P(k) is the discrete version of G, and w(k) is the 
white system noise vector. The transition matrix, (k), 
relates the state vector at time t,, x(k), to the state 
vector at the next sampling time, x(k+1), under noiseless 
conditions. 


Estimates of the state vector, (kK), are obtained by 


filtering the measurements, z(k). At each sample time, t,, 
a new measurement is taken, which can be represented by 


Z(k) = H(k) x(k) * y(k) , (4.12) 


where H(k) is the observation matrix, and V(k) is the 
measurement noise. 
For GPS-aiding, the measurements are the errors in 


attitude, position, and velocity. Therefore, 
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ERIKO). [0]. (0) [0] 
[ONT [o] (0) [0] 
= TONI GN T] [ONTOJ „ (4.13) 
[0] [0] [0] [0] [0] 
Aa) [ol [ol [0] Toh 


and v(k) consists of the noise on the aiding systems, 
including GPS, the depth meter, and the compass. 

In order to use a Kalman filter, the noise processes, 
w(k) and v(k), must be uncorrelated. In addition, the non- 
zero, diagonal components of the system and measurement 
noise covariance matrices, Q(k) and R(k), must be known. 

For AUV applications, these conditions are met, but the 
covariances differ depending on the conditions and equipment 
used. The best accuracy is obtained with a high-grade INS 
aided by differential GPS. 


The recursive Kalman filter algorithm is (Ref. 3:p. 235] 


K(k) » P"(kK) HT[HP" (X HT*R(k)]^t 
AR) ECO) EKI KP [ZIK) “HX” (k)] 
P(k) = [[I] -K(k)H] P-(k) (4.14) 
P7(k+1) = (k) P(k) 87 (Kk) *Q(X) 
2 (k+1) =¢(k)£(k) , 


where the minus sign superscript refers to conditions just 
prior to incorporating the measurement. The K(k) matrices 
are the Kalman gains and the P(k) matrices are the error 


covariance matrices, based on the estimation errors given by 


A KE (4.15) 
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This algorithm requires an initial estimate of P(0) and 


£-(0). Since the INS errors are zero mean, the initial 


estimates of the state variables, £-(0), are zeros. The 


values used for the initial variances in the error 
covariance matrix, P'(0), are based on a commercially 
avallable INS. This particular INS was designed for 
civilian aircraft and measures position with an accuracy of 
two nautical miles per hour (20 = 2 nmi/h) (Ref. 18:p.24]. 


The initial variances are listed in Table 4.1. 


Table 4.1: INITIAL VARIANCES IN THE ERROR COVARIANCE MATRIX 
(After Ref. 18:pp. 63-64) 


= 


| STATE VARIABLE 
£ 


INITIAL ERROR VARIANCE 
2 35 Tx 9 ND (radians)“ 
= 400 = (meters)” | 









| 





O 
< 


Nor (meters/second)? 


2.35 x 10?  (radians/second)? 
iio? (meters/second?)? 
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V. COMPUTER SIMULATION 


A computer simulation has been conducted to verify the 
theory explained in the preceding sections. As illustrated 
in Figure 5.1, this simulation included three malin parts: 
an AUV model, an INS error model, and a Kalman filter. The 
simulation software includes sections that perform 
additional functions, such as providing control inputs to 
the AUV model, and adding noise to the measurements. This 
software was developed using the MATLAB applications 


package. 


COMPUTER SIMULATION 


CONTROL FORCES AND NOISY INS ERROR 


INPUTS ROTATION RATES MEASUREMENT 35 ESTIMATES 
KALMAN 


FILTER 





Figure 5.1: Block diagram of computer simulation. 


An existing, nonlinear AUV model [Ref. 19:pp. 18-34] was 
used to create realistic sequences of forces and rotations. 
This model, AUV2, was written in the C software language and 
compiled for use with MATLAB. The AUV2 program is based on 
the parameters of the Naval Postgraduate School’s second 
AUV. 
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The flight profile for the AUV consisted of an initial 
surface interval followed by a series of dives. The initial 
surface interval was a two-minute period during which the 
AUV remained near the surface for continuous GPS 
measurements. This was done to allow the Kalman filter to 
settle down. A reference depth of one half meter was used 
because the AUV2 model is only valid when the vehicle is 
submerged. The hydrodynamic equations are different for a 
vehicle on the surface. The reference depth was used ina 
feedback loop, along with a gain term, to set the angle of 
the AUV’s bow and stern planes. Several trial runs were 
conducted to find a suitable value for the gain. 

Following the initial surface interval, the AUV model 
was given a sinusoidal input to simulate thirty dives. Each 
dive lasted for two minutes, and a GPS measurement was made 
at the end of each dive. 

The forces and rotations output from AUV2 are used by an 
INS error model to create noisy measurements. This error 
model is based on the equations in Chapter 4 and is 
illustrated in Figure 5.2. 

To initialize the transformation matrix, R%, it is 
assumed that the AUV is aligned with the local-level frame. 
Thus the transformation matrix from body to local-level 
coordinates, R‘,, is reduced to the identity matrix, [I], 


and, from Equation 3.7, we have R, = R,. A latitude and 


n 
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Figure 5.2: Block diagram of INS error model. 


longitude near Monterey, California was used to initialize 
R^ in accordance with Equation 2.10. Equation 2.11, based 
on the body rotations, is used to update R^. 

The forces are used, along with Rẹ, to update the error 
dynamics matrix, [F]. The system noise matrix, G, is then 
used, along with [F], to form the discrete-time version of 
the state equation. To convert Equation 4.9 into Equation 
4.11, the following approximations are useful (Ref. 3:p.224] 


and (Ref. 20]: 
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$ = [I] + Pee ELL uL 
2! (5.1) 


T = [[7] (Aĉ) plan Mano sas e 


The MATLAB command "c2d" also converts the state 
equation from the continuous to the discrete verslon. A 
comparison was done that showed that the approximating 
equations ran faster than the "c2d" algorithm. Several runs 
were executed to find out how many terms were required in 
Equations 5.1 to reduce the approximation errors to 
negligible values. 

Using MATLAB’s random number generator, and specifying a 
normal distribution with a given standard deviation, system 
noise is added to create a simulated state vector. 
Similarly, measurement noise is added and the observation 
matrix is used to create a simulated measurement vector. 
These noisy measurements are then fed into the Kalman filter 
to estimate the simulated state vector. 

The values used for the standard deviations of the 
system noise were 0.01 deg/h for gyro drifts and 10 mgal for 
accelerometer biases [Ref. 18:p. 64]. These values were 


based on an INS that uses RLG’s. Converting the units gives 


Og = e Ea EA = 5x10 ?rad/s 


hour À 180 — (5.2) 
0, = (10x10ga1)|0-02m/5* = 10 "m/s? 
gal 
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For the measurement noise, the standard deviations were 
based on GPS statistics for position and velocity, and two 
levels of accuracy for attitude measurements. For 
differential GPS, the standard deviations of position 


measurement noise, o0,, is 2.5 meters, and the standard 


pf 


deviation of velocity measurement noise, o,, is 0.05 meters 


v,' 


per second. For the C/A code GPS simulation, 0, = 50 m, and 


o, = 0.9 m/s. 
A standard deviation of 10° radians was used to simulate 
a high accuracy attitude measurement. A low accuracy 
Simulation was run using a value of 0.1 radian. 
These standard deviations were squared to obtain the 


Variances required for the measurement noise covariance 


matrix, R. Table 5.1 lists the system noise variances. 


Table 5.1: STATE VARIABLE VARIANCES (After Ref. 18:p. 65) 


SYSTEM NOISE VARIANCE | 





| STATE VARIABLE 
attitude error sorlo "Eradlans)“ | 
o (meters)? | 













velocity error OS (meters/second)? 


gyro drift … 3 x 10”? (radians/second)? 
3.7 x 107 


A correlation time of 40 hours was used for the Gauss- 








(meters/second?)? 





Markov process, i.e., gyro drifts and accelerometer biases 
(Ref. 18:p. 65]. 
Several MATLAB command files ("m-files") were generated 


to perform the separate tasks of the computer simulation. 
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These files are included in Appendix B. The file 
"auv2surf.m" calls the AUV2 model with control inputs to 
generate vehicle state outputs for the surface interval. 
The control inputs are bow and stern plane angle commands 
based on the difference between a reference depth of 0.5 
meters and the fedback depth output from AUV2. The file 
"auv2dive.m" calls AUV2 with a sinusoidal input to simulate 
a series of dives. The file "plotdpth.m" graphs the depth 
during the first five dives. 

The files "simlsurf.m", "sim2surf.m", "sim3surf.m", and 
"sim4surf.m" use AUV2 output states during the surface 
interval to simulate the INS errors and to generate error 
estimates with the Kalman filter. The only difference in 
these four files are the values used for the standard 
deviations of position, velocity, and attitude measurement 
noise. The file "plotl.m" graphs the results from the 
simulated surface interval runs. 

The file "simdives.m" uses the results from the surface 
runs and "auv2dive.m" to simulate the series of dives. The 
file "plot2.m" graphs the results of the dive runs. The 
file "esterr.m" calculates and plots the INS error estimate 
errors following each simulated series of dives. 

After the simulation software was debugged, and 
observability was verified, four runs were executed to 
compare the combinations of differential or C/A code GPS 


With high-accuracy or low-accuracy attitude measurements. 
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Table 5.2 lists the combinations used for the simulations. 
The results of the four simulation runs are presented and 


discussed in the next chapter. 





Table 5.2: ACCURACY LEVELS DURING THE COMPUTER SIMULATIONS 


[RNA | ATTITUDE MEASUREMENTS | 
high accuracy 
3 
4 










| differential low accuracy 
EE re ee ode high accuracy 
Pt [|  C/A code 
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VI. RESULTS 


The results from the four simulation runs have been 
graphed using MATLAB plotting commands. The AUV depth 
during the initial surface interval and the following series 
of dives were the same for all four runs. Figure 6.1 shows 
the depth during the surface interval. Figure 6.2 shows the 


depth during the first five dives. 
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150 
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Figure 6.1:  AUV depth during initial surface interval. 
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Figure 6.2: AUV depth during first five dives. 
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Figures 6.3 through 6.17 show the results from the first 
run. Figures 6.18 through 6.62 show the results from the 
next three runs. The first five figures from each run show 
the output states during the surface interval. The next 
five figures show the output states during the dive series. 
These output states include the modelled INS errors and the 
INS error estimates from the Kalman filter. The dashed 
curves are the error estimates. The last five figures in 


each set show the error estimate errors. 
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Figure 6.3:  First-run, surface-interval attitude error. 
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Figure 6.4: First-run, surface-interval position error. 
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Figure 6.5:  First-run, surface-interval velocity error. 
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Figure 6.6: First-run, surface-interval gyro drift. 
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Figure 6.7: First-run, surface-interval accelerometer bias. 
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Figure 6.10: 
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First-run, dive-series attitude error. 
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First-run, dive-series position error. 
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First-run, dive-series velocity error. 
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Figure 6.11: First-run, dive-series gyro drift. 
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Figure 6.12: First-run, dive-series accelerometer bias. 
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Figure 6.13:  First-run attitude error estimate error. 
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Figure 6.14: First-run position 
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Figure 6.15: First-run velocity 
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Figure 6.16: First-run gyro drift estimate error. 
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Figure 6.17: First-run accelerometer bias estimate error. 
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Figure 6.18: Second-run, surface-interval attitude error. 
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Figure 6.19: Second-run, surface-interval position error. 
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Figure 6.20: Second-run, surface-interval velocity error. 
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Figure 6.21:  Second-run, surface-interval gyro drift. 
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Figure 6.22: Second-run, surface-interval accelerometer bias. 
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Figure 6.23: Second-run, dive-series attitude error. 
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Figure 6.24: Second-run, dive-series position error. 
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Figure 6.25: Second-run, dive-series velocity error. 
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Figure 6.26: Second-run, dive-series gyro drift. 
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Figure 6.27: Second-run, dive-series accelerometer bias. 
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Figure 6.28: Second-run attitude error estimate error. 
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Figure 6.35: MThird-run, surface-interval velocity error. 
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Figure 6.36: Third-run, surface-interval gyro drift. 
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Figure 6.37: Third-run, surface-interval accelerometer bias. 
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Figure 6.38:  Third-run, dive-series attitude error. 
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Figure 6.39:  Third-run, dive-series position error. 
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Figure 6.40: Third-run, dive-series velocity error. 
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Figure 6.41: Third-run, dive-series gyro drift. 
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Figure 6.42: Third-run, dive-series accelerometer bias. 
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Figure 6.43: Third-run attitude error estimate error. 
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Figure 6.44: Third-run position error estimate error. 
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Figure 6.45:  Third-run velocity error estimate error. 
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Figure 6.46:  Third-run gyro drift estimate error. 
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Figure 6.47: Third-run accelerometer bias estimate error. 
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Figure 6.48:  Fourth-run, surface-interval attitude error. 


position error, melers 
position error, melers 


| 

“ 
O 
0 


time, seconde 


POSITION ERROR 
LII 


1OO 


time, seconds 





Figure 6.49:  Fourth-run, surface-interval position error. 
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Figure 6.50:  Fourth-run, surface-interval velocity error. 


GYRO DRIFT GYRO ORIFT 


gyro drift, rodions/second 


100 


time, seconde time, seconde 


GYRO DRIFT 


gyro drift, radians/second —— qyro drift, radians/second 





Figure 6.51:  Fourth-run, surface-interval gyro drift. 
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Figure 6.52: Fourth-run, surface-interval accelerometer bias. 
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Figure 6.53:  Fourth-run, dive-series attitude error. 
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Figure 6.54:  Fourth-run, dive-series position error. 
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Figure 6.55: Fourth-run, dive-series velocity error. 
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Figure 6.56:  Fourth-run, dive-series gyro drift. 
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Figure 6.57: Fourth-run, dive-series accelerometer bias. 
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Figure 6.58:  Fourth-run attitude error estimate error. 
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Figure 6.59:  Fourth-run position error estimate error. 
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Figure 6.60:  Fourth-run velocity error estimate error. 


140 x19-8 GYRO DRIFT EST. ERROR GYRO DRIFT EST. ERROR 


o 
— 10 
—20 


1o 20 10 20 


dive number dive number 


x<107S GYRO ORIFT EST. ERROR 


10 20 


dive number 





Figure 6.61:  Fourth-run gyro drift estimate error. 
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Figure 6.62: Fourth-run accelerometer bias estimate error. 


Conclusions based on these results are included in the 


next chapter. 
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VII. CONCLUSIONS AND RECOMMENDATIONS 


This research has produced a computer model of the 
integration of GPS and INS measurements for the navigation 
of an AUV. Noisy INS measurements were simulated with a 
model based on a medium-grade INS that uses RLG’s. Noisy 
GPS and attitude measurements were simulated and subtracted 
from the modeled INS measurements to simulate INS errors. 
Kalman filtering was used to estimate those errors. Four 
simulations were run using combinations of high and low 
accuracy GPS and attitude measurements. 

The results from the simulation runs matched the 
expectations. This can be observed by comparing Figure 6.9 
with Figure 3.4. The simulated position error after one 
hour is on the order of one nautical mile, which is what is 
predicted by the theoretical equations. 

The simulation shows that Kalman filtering can estimate 
the position and velocity errors to the expected accuracy 
levels. Figures 6.14, 6.15, 6.29, and 6.30 show that the 
accuracy of the position and velocity error estimates is 
independent of the accuracy of the attitude measurements. 

The computer model can be used to compare combinations 
of INS's and GPS equipment with different levels of 
accuracy. These comparisons might include low-grade or 


high-grade INS’s and P-code GPS receivers. 
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The recommended approach to continuing this research 
would begin with a verification of the INS error model. 
This could be accomplished using the AUV in the pool at the 
Naval Postgraduate School. A trajectory measurement using 
the AUV’s sonars could be used as a standard to determine 
typical INS errors. Simulations would need to be run with a 
profile compatible with the dimensions of the pool. 

Additional research could investigate implementation 
with closed-loop utilization of state estimates and 


smoothing for improved accuracy. 
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APPENDIX A. GPS DATA PROCESSING SOFTWARE 


unpack. for: 


100 


50 


60 


500 


unpack mx4200 data 


characterx11 Rtype 

character*14 fname, outfname 

write (*,’(a\)’) ' What is the input file name? ” 
read (*,’(a)’) fname 

open (1, file=fname, access=’sequential’, 


+status=’old’) 


write (*,'(aM)') ' What is the output file name? ” 
read (*,'(a)') outfname 
open (2, file-outfname, status-'new') 
StartTIME = 0 
incH = 0 
incLAT = 0 
incLONG = O 
continue 
read (1, (a)” end=500,err=100) Rtype 
write(*,’(a)’) Rtype 
if (Rtype .eq. 'SPMVXG,OOl,”) then 
backspace 1 
read (1,50,err=100) Rtype,ih,im,is,ideg,dmin, 


+ideglong,dminlong,alt,isrc 


format (a,312,1x,12,f6.3,3X,137t003,3X,L 701 MN 
seconds = im * 60 + is 
if (startTIME .eq. 0) then 
startTIME = seconds 
iho = ih 
endif 
if (ih .gt. ih0) incH = 3600 k (1h-ihO) 
itime = incH + (seconds - startTIME) + 1 
if (ideg .ne. 34) incLAT = 60 * (ideg - 34) 
delLAT = incLAT + (dmin - 6.733) 
if (ideglong .ne. 119) incLONG = 60*(ideglong - 119) 
delLONG = incLONG + (dminlong - 6.9564) 
delALT = alt - 12.71 
write (2,60) itime,delLAT,delLONG,delALT,isrc 
format (1xX,16,5x,f6.3,5bx/, F6.3 , 9X “EON ox Iel) 
endif 
goto 100 
close (1) 
close (2) 
end 
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unpkvel.for: 


qaqa aa 


100 


50 


60 


500 


Steve Nagengast 


filename:  unpkvel.for 


unpack mx4200 velocity data 


character*11 Rtype 


character*14 fname, 


write (*,’(a\)’) 


read (*,’(a)’) fname 


open (1, file=fname, 


tstatus-'/old') 
weite (ay) 


' What is the output file name? 


read (*,'(a)') outfname 
open (2, file-outfname, 


continue 


outfname 
' What is the input file name? 


access-'sequential', 


Status-'new') 


read (1,’(a)’,end=500) Rtype 
write(*,’(a)’) Rtype 
'SPMVXG,011,") then 


if (Rtype .eq. 
backspace 1 


read (1,50) Rtype,hdng, spd 
Rormat (a;f5 1,IX,E5.1) 
write (2,60) hdng,spd 
format (1x,f5.1,5x,f5.1) 


endif 
goto 100 
close (1) 
close (2) 
end 
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APPENDIX B. COMPUTER SIMULATION SOFTWARE 


auv2surf.m: 


simulates AUV2 for initial 
two-minute surface interval 


% Steve Nagengast 
% filename:  auv2surf.m 


o? o9 


$ Creates output file from AUV2 model 
Clear 
$ initial surface interval (120 seconds = two minutes) 


desireddepth = 0.5; 
planegain = 0.008; 


idt = 1; 
rpm = 550; 
state(: 1 = zeros(12, 1); 


for k=1:120/idt 
plane = planegain * (desireddepth - state(9,k)); 
if (plane > 0.7) plane = 0.7; end 
if (plane < -0.7) plane = -0.7; end 
inpt = (0; O; plane; -plane; rpm]; 
oldstate = state(:,k); 
state(:,k+1) = auv2(oldstate, inpt, idt); 
end 
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$ save and plot results from end of initial surface interval 
save srfstate state 


plot(-state(9,:)) 


auv2dive.m: 


Steve Nagengast 
filename: auv2dive.m 


simulates AUV2 thru thirty dives 


o? oo 
o? o 


o 


; reload last state from initial surface interval 


Clear 
load srfstate 
stait(: 1) = state PE. 


clear state 
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$ run through thirty dives 


numbdives = 30; idt = l; T = 120; 
rpm = 550; nn = 0; 
for k2=1:numbdives 
for jj=1:T/idt 
numb = (k2-1-nn)*T + jj; 
plane = 0.02 * sin(pi*j 3/60); 
inpt = (0; 0; plane; -plane; rpm]; 
oldstate = stait(:,numb); 
stait(:,numb+1) = auv2(oldstate, inpt, idt); 
end 
if (k2==5) 
nn = k2; statemp=stait(:,numb“1); 
save statel stait 
clear stait 
stait(:,1) = statemp; 
end 
if (k2==10) 
nn = k2; statemp-stait(:,numb-1); 
save state2 stait 
clear stait 
stait(:,1) = statemp; 
end | 
if (k2==15) 
nn = k2; statemp=stait(:,numb-1); 
Save state3 stait 
clear stait 
stait(:,1) = statemp; 
end 
if (k2==20) 
nn = k2; statemp=stait(:,numb+1); 
Save state4 stait 
clear stait 
stait(:,1) = statemp; 
end 
if (k2==25) 
nn = k2; statemp=stait(:,numb+1); 
Save state5 stait 
clear stait 
stait(:,1) = statemp; 
end 
if (k2==30) 
nn = k2; statemp-stait(:,numb-1); 
Save state6 stait 
clear stait 
stait(:,1) = statemp; 
end 
end 
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plotdpth.m: 


$ Steve Nagengast % plots the depth during first five dives 
$ filename: plotdpth.m 


clear 
load statel $ from auv2dive.m 
!del divedpth.met 
numbdives = 5; 
T = 120; 
idt = 1; 
for k2 = 1:numbdives 
for jj = 1:T/idt 
numb = jj + (k2-1) * T; 
divedist (numb) = stait(7,numb); 
divedepth (numb) = -stait(9,numb); 
end 
end 


3 plot results 


cig 
divemin =1/60:1/60: (numbdives) *120/60; 
=numbdives; 


subplot(211), plot(divemin,divedepth) ; 
title(’DEPTH vs. TIME’); 

xlabel(’time, minutes’); 
ylabel(’depth, meters’); 

Guia 

subplot(212), plot(divedist,divedepth) ; 
title(“DEPTH vs. DISTANCE’); 

Xlabel (distance, meters’); 
ylabel(’depth, meters’); 

grid 

meta divedpth; 


simisurf.m: 


Steve Nagengast $ simulates INS/GPS Kalman filter for 
filename: simisurf.m $ initial 2-minute surface interval 


AN oe 


Estimates gyro drift errors and accelerometer biases 
ln body coordinates. Also estimates errors in attitude, 
position, and velocity in earth coordinates. 


o? o9? o? 


oo 


initialize state, estimated state, and covariance matrices 


clear 

x = Zeros (15,137 

xhat = zeros(15,1); 
prexhat = zeros(15,1); 
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$ initial variances in P(0) matrix 
prep - zeros(15); 


prep(1,1) = 2.35e-7; $ radians^2 
prep(2,2) = 2.35e-7; % radians”2 
prep(3,3) = 2.35e-7; $ radians^2 
prep(4,4) = 400; $ meters^2 
prep(5,5) = 400; % meters"2 
prep(6,6) = 400; $ meters^2 
prep(7,7) = le-8; % (meters/second) *2 
prep(8,8) = le-8; % (meters/second) *2 
prep(9,9) = le-8; % (meters/second) *2 
prep(10,10) = 2.35e-15; % (radians/second) *2 
prep(11,11) = 2.35e-15; $ (radians/second)^2 
prep(12,12) = 2.35e-15; $ (radians/second)^2 
prep(13,13) = le-8; $ (meters/second^2)^2 
prep(14,14) = le-8; $ (meters/second^2)^2 
prep(15,15) = 1e-8; $ (meters/second^2)^2 
H = zeros(9,15); matrix 
for j=l:9 

E) Sl; 
end 


$ initial transformation matrix from body to earth 
$ coordinates 
$ (assuming body is aligned with local-level frame) 


lat - 0.64; % radians north (vicinity Monterey) 

long = pi - 2.13; % radians east (vicinity Monterey) 

Rbe = [-sin(lat)*cos(long) -sin(long) -cos(lat)*cos (long) 
-sin(lat)*sin(long) cos(long) -cos(lat)*sin(long) 
cos (lat) 0 sin(lat) Jt 


% initialize noise 
rand('normal'); 
W — Zeros(6,1); 
V — Zeros(9,1); 


% standard deviations 


Sigmadrift = 1.74e-6; % radians/second 
sigmabias = 6.le-4; % meters/second 
sigmaatt = le-6; % radians 
Sigmaposit = 2.5; $ meters 
sigmavel = 0.05; % meters/second 
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vardrift 
varbias 
varatt 
varposit 
varvel 
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$ variances 


sigmadrift^2; 
sigmabias^2; 
sigmaatt^2; 
sigmaposit^2; 
sigmavel^2; 


% Q matrix 


O 


Q = zeros(15); 


Q(1,1) 
Q(2,2) 
Q(3,3) 
Q(7,7) 
Q(8,8) 
Q(9,9) 
Q(10,10) 
Q(11,11) 
QNM) 
QUSS) 
Q(14,14) 
O(15, 15) 


2539 lOS, 
os 99e=E2) 
oe oe 2, 
le-6; 

le-6; 

le-6; 

3e-12; 
36-127 
3e-12; 
3 76=7; 
Suus 
Sue (6 


£ R matrix 


R = Zeros(9); 


R(1,1) 
R(2,2) 
R(3,3) 
R(4,4) 
R(5,5) 
R(6,6) 
R(7,7) 
R(8,8) 
R(9,9) 


mr Ea 
omegaE = 
SkewE = 


Tgyro 
Tacc 
null3 
G = (nul 
nul 
nul 
eye 
nul 


varatt; 
varatt; 
varatt; 
varposit; 
varposit; 
varposit; 
varvel; 
varvel; 
varvel; 


lize submatrices for state dynamics matrix (F) 


N 


0.7292115e-4; $ radians/second 

[ O omegaE 0 

-omegaE 0 0 

O 0 O 

-eye(3) / (3600*40); 

-eye(3) / (3600*40); 
zeros(3); 


]; 
1 / (correlation time) 
1 / (correlation time) 


oe oe 


13 nul13 
13 null3 
ES RUDI 
(3)*vardrift null3 
13 eye(3)*varbias ]; 
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$ initial surface interval (120 seconds = two minutes) 


load srfstate $ from auv2surf.m 
for k=1:120 

f = (state(:,Krl) - state(:,k)); 

dist(k) = state(7,k+1); 

depth(k) = -state(9,k+1); 


% update state dynamics matrix 


sbx = state(4,k+1) - Rbe(3,1) * omegaE; 
sby = state(5,k+1) - Rbe(3,2) * omegaE; 
sbz = state(6,k+1) - Rbe(3,3) * omegaE; 
skewb = [ O -sbz sby 
sbz O -sbx 
-sby sbx (l 


Rbe = Rbe + Rbe * skewb; 
fb = f(1:3); fe = Rbe k fb; 


skewf = [ O fe(3) -fe(2) 
-fe(3) 0 fe(1) 
fe(2) -fe(1) O ); 


F = [skewE null3 null3 Rbe null3 
null3 null3 eye(3) null3 null3 
skewf null3 2*skewE null3 Rbe 
nudi null3 null3 Tgyro null3 
Durr null3 null3 null Tacc J; 


% approximate phi and gamma (idt = 1) 


F2 = F^2; 
F3 = F2 * F; 
F4 = F3 * F; 


phi = eye(15) + F + F2/2 + F3/6; 
gamma=(eye(15) + F/2 + F2/6 + F3/24 + F4/120) * G; 


o 


% make some noise and create noisy measurements 


for n=1:6 

w(n) = rand; 
end 
v(1) = rand * varatt; 
v(2) = rand * varatt; 
v(3) = rand * varatt; 
v(4) = rand * varposit; 
v(5) = rand * varposit; 
V(6) = rand * varposit; 
V(7) » rand * varvel; 
v(8) = rand * varvel; 
V(9) = rand * varvel; 


X(:,k*1) - phi * x(:,k) + gamma * w; 
as Hk (k) + v; 
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$ Kalman filter 


K = prep * H’ * inv(H * prep * H’ + R); 
xhat(:,k) = prexhat + K * (z ~ H * prexhat); 
p (eye pes 
prep - phi * p * phi' HQ; 
prexhat - phi * xhat(:,k); 
countdown = 120 - k 

end 


% save and plot results from end of initial surface interval 


divex = x(:,121); stait = state(:,121); 

save snl divex stait Rbe omegaE skewE Tgyro Tacc H prep R Q 
prexhat G 

save vars varatt varposit varvel 

plotl 


ploti.m: 


3 Steve Nagengast % plots results of surface interval 
% filename: plotl.m 


ldel meta?.met 
time = 1:120; min = 1/60;1/60120/60, e 


subplot(211), plot(min,depth); title(’DEPTH vs. TIME’); 
xlabel('time, minutes’); ylabel(’depth, meters’); grid 
subplot(212), plot(dist,depth); title(’DEPTH vs. DISTANCE"); 
xlabel(’distance, meters’); ylabel(’depth, meters’); 

grid; meta metal; clg 


subplot(221), plot(time,x(1,1:120),/-/,time,xhat(1,:), /-~/) 
title(’ATTITUDE ERROR’); xlabel(’time, seconds’); 
ylabel(’attitude error, radians’); grid 

subplot(222), plot(time,x(2,1:120),/-/,time,xhat(2,:), /--/) 
title('ATTITUDE ERROR’); xlabel(’time, seconds’); 
ylabel('attitude error, radians’); grid 

subplot(223), plot(time,x(3,1:120),'-',time,xhat(3,:),"'--') 
title('ATTITUDE ERROR’); xlabel(’time, seconds’); 
ylabel(’attitude error, radians’); grid; meta meta2; clg 


subplot(221), plot(time,x(4,1:120),/-/,time,xhat(4,:), /--/) 
title(’POSITION ERROR’); xlabel(’time, seconds’); 
ylabel(’position error, meters’); grid 

subplot(222), plot(time,x(5,1:120),“=SMtime,xhaE(5,:)) == 
title(’POSITION ERROR’); xlabel(’time, seconds’) ; 
ylabel(’position error, meters’); grid 

subplot (223), plot(time,x(6,1:120),’-’,time,xhat(6,:),’=--') 
title(’POSITION ERROR’); xlabel(’time, seconds’) ; 
ylabel(’position error, meters’); grid; meta meta3; clg 
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Eulpbeloc(227), plot(time,x(7,1:120),/-',time,xhat(7,:),/--/) 
title(’VELOCITY ERROR’); xlabel(’time, seconds’); 
ylabel(’velocity error, meters/second’); grid 

ENboloE(222) plot(time,x(8,1:120),/-',time,xhat(8,:),/--/) 
title(’VELOCITY ERROR’); xlabel(’time, seconds’); 
ylabel(’velocity error, meters/second’); grid 

ERpeolor(223), plot(time,x(9,1:120),/-/',time,xhat(9,:),/--/) 
title(’VELOCITY ERROR’); xlabel(’time, seconds’); 
ylabel(’velocity error, meters/second’); grid; meta meta4; 
clg 


subplot(221), 
pEioE(time,x(l0,1:120),7-',time,xhat(10,:),/--/) 
title(’GYRO DRIFT’); xlabel(’time, seconds’); 
ylabel(’gyro drift, radians/second’); grid 
subplot(222), 
plot(time,x(11,1:120),/-/',time,xhat(11,:), /--/) 
title(’GYRO DRIFT’); xlabel(’time, seconds’); 
ylabel(’gyro drift, radians/second’); grid 
subplot(223), 
pEKOE(tlme,x(12,1s120),/-',time,xhat(12,:),/--/) 
title(’GYRO DRIFT’); xlabel(’time, seconds’); 
ylabel(’gyro drift, radians/second’); grid; meta met5; clg 


subplot(221), 
puot(tuime;x(13,1:120),'-',time,xhat(13,:),"'--') 
title(’ACCELEROMETER BIAS’); xlabel(’time, seconds’); 
ylabel(’acc. bias, meters/second*2’); grid 
subplot(222), 
pEocr(time,x(14,1:120),/-/',time,xhat(14,:),/--/) 
title(’ACCELEROMETER BIAS’); xlabel(’time, seconds’); 
ylabel(’acc. bias, meters/second*2’); grid 
subplot(223), 
plot(time,x(15,1:120),'-',time,xhat(15,:),'--') 
title(’ACCELEROMETER BIAS’); xlabel(’time, seconds’) ; 
ylabel(’acc. bias, meters/second^2'); grid; meta met6; 


Simdives.m: 


Steve Nagengast % Simulates INS/GPS thru thirty dives 
filename: simdives.m 


o? oe 


oo 


(uses average forces and rotation rates) 


oo 


reload data from initial surface interval 
clear 


load snl 
load vars 
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% run through thirty dives 


numbdives = 30; idt = 1; T = 120; 
null3 = zeros(3); 
w = zeros(6,1); 
V zeros(9,1); 
9; c (p w Jm 
totalf = zeros(3,1); 
totalrot = zeros(3,1); 
load statel 
state = stait; 
clear stait 
clear statel 
nn = 0; 
for k2=1:numbdives 
for jj=1:T/idt 
numb = (k2-l-nn) * T + jj; 
f = (state(:,numb+1) - state(:,numb)) / iat; 
totalrot = totalrot + state(4:6,numb+1); 
totalf = totalf + f(1:3); 
end 
if (k2==5) 
nn = k2; 
clear state 
load state2 
state = stait; 
clear stait 
clear state2 


oe 


from auv2dive.m 


k2 = 5; 
nn = k2; 
end . 
if (k2==10) 
nn = k2; 


clear state 
load state3 
state = stait; 
clear stait 
clear state3 


k2 = 10; 
nn = k2; 
end 
if (k2==15) 
nn = K2; 


clear state 
load state4 
state = stait; 
clear stait 
clear state4 


k2 = 15; 
nn = k2; 
end 
if (k2==20) 
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nn k2; 

clear state 
load state5 
state stait; 
clear stait 
clear state5 


k2 = 20; 
nn = k2; 
end 
if (k2==25) 
nn = k2; 


clear state 
load state6 
state stait; 
clear stait 
clear state6 


k2 = 25; 
nn = k2; 
end 


9 


$ update transformation matrix and forces 


averot 
aveforce 
sbx 


potalrot T; 
cotali FTT; 
averot(1) - Rbe(3,1) * omegaE; 


sby averot(2) - Rbe(3,2) * omegaE; 

sbz = averot(3) - Rbe(3,3) * omegaE; 

skewb = [ 0 -sbz sby 
sbz 0 -sbx 
-sby sbx o 

Rbe = Rbe + Rbe * skewb * T; 

fe = Rbe * aveforce; 

% update state dynamics matrix 

skewf = [ 0 fe(3) -fe(2) 

-fe (3) 0 fe(1) 
fe(2) -fe(1) o jJ; 

F = (skewE null3 null3 Rbe null3 
null3 null3 eye(3) null3 null3 
skewf null3 2*skewE null3 Rbe 
null3 null3 null3 Tgyro null3 
null3 null3 null3 null? Tacc 1; 

% approximate phi and gamma (T = 120) 

F2 = F^2; 

F3 = F2 >* F; 

F4 = F3 * F; 

phi = eye(15) + F*T + F2*(T"2)/2 + F3*(T"3)/6; 


gamma=(eye(15) + F*T/2 + F2*(T"2)/6 + F3*(T^3)/24 + 
F4*(T^4)/120)*T*G; 
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9 


% make some system noise and update state 


for n=1:6 
w(n) = rand; 
end 
divex(:,k2+1) = phi * divex(:,k2) + gamma * w; 


% make some measurement noise 
v(1) = rand * varatt; 

v(2) = rand * varatt; 

v(3) = rand * varatt; 

v(4) = rand * varposit; 

v(5) = rand * varposit; 

v(6) = rand * varposit; 

V(7) = rand * varvel; 

V(8) = rand * varvel; 

v(9) = rand * varvel; 


% create noisy measurements and run through Kalman filter 


2 = H * divex(:,k2) + v; 
K = prep * H' * inv(H * prep * H’ + R); 
divexhat(:,k2) = prexhat + K * (z - H * prexhat); 
p = (eye(E5) =- Kos hee Prep, 
prep = phi * p * phi’ + Q; 
prexhat = phi k divexhat(:,k2); 
divecountdown = numbdives - k2 
end 


9 


$ save and plot results from dives 


save sn2 divex divexhat numbdives 


plot2 

plot2.m: 

$ Steve Nagengast % plots results from one hour of 
dives 


9 


$ filename: plot2.m 


clear; clg 

pack 

load sn2 

ldel met2?.met 

divemin = 2:2: (numbdives) *2; 
n=numbdives; 
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subplot(221), 
ENOE(divenan,divex(1,1:n),”-/',divemin,divexhat(1,:),/--”/) 
title(’ATTITUDE ERROR’); xlabel(’time, minutes’); 
ylabel(’attitude error, radians’); grid 

subplot(222), 
plot(divemin,divex(2,1:n),/-/',divemin,divexhat(2,:),/--”) 
title(’ATTITUDE ERROR’); xlabel(’time, minutes’) ; 
ylabel(’attitude error, radians’); grid 

subplot(223), 
plot(divemin,divex(3,1:n),'-',divemin,divexhat(3,:),'--'") 
title('/ATTITUDE ERROR’); xlabel(’time, minutes’); 
ylabel(’attitude error, radians’); grid; meta met22; clg 


Supplot(221), 
plot(divemin,divex(4,1:n),/-/',divemin,divexhat(4,:), /--/) 
title(’POSITION ERROR’); xlabel(’time, minutes’); 
ylabel(’position error, meters’); grid 

subplot(222), 

plot (divemin, divex(5,1:n),’-’,divemin, divexhat(5,:),’--’) 
title(’POSITION ERROR’); xlabel(’time, minutes’); 
ylabel(’position error, meters’); grid 

subplot(223), 
plot(divemin,divex(6,1:n),'-',divemin,divexhat(6,:),"'--'") 
title(’POSITION ERROR’); xlabel(’time, minutes’); 
ylabel(’position error, meters’); grid; meta met23; clg 


subplot(221), 
plot(divemin,divex(7,1:n),'-',divemin,divexhat(7,:),"'--"') 
title(’VELOCITY ERROR'); xlabel('time, minutes'); 
ylabel('velocity error, meters/second'/); grid 
subplot(222), 
plot(divemin,divex(8,1:n),'-',divemin,divexhat(8,:),'--') 
title('VELOCITY ERROR’); xlabel(’time, minutes’); 
ylabel('velocity error, meters/second’); grid 
subplot(223), 
plot(divemin,divex(9,1:n),'-',divemin,divexhat(9,:),'--"') 
title(’VELOCITY ERROR’); xlabel(’time, minutes’); 
ylabel(’velocity error, meters/second’); grid; meta met24; 
clg 


subplot(221), 
plot(divemin,divex(10,1:n),'-',divemin,divexhat(10,:),"'--") 
title(’GYRO DRIFT’); xlabel(’time, minutes’); 

ylabel(’gyro drift, radians/second’); grid 

subplot(222), 
plot(divemin,divex(11,1:n),'-',divemin,divexhat(11,:),'--') 
title(’GYRO DRIFT’); xlabel(’time, minutes’); 

ylabel('gyro drift, radians/second’); grid 

subplot(223), 
plot(divemin,divex(12,1:n),/-/,divemin,divexhat(12,:), /--/) 
title('GYRO DRIFT/); xlabel('time, minutes'); 

ylabel(’gyro drift, radians/second’); grid; meta met25; clg 
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SUbpIOE C221) 7 
plot(divemin,divex(13,1:n),/-',divemin,divexkiat[ 13), =) 
title(’ACCELEROMETER BIAS’); xlabel(’time, minutes’); 
ylabel(’acc. bias, meters/second*2’); grid 

subplot(222), 
plot(divemin,divex(14,1:n),/-/,divemin,divexhat(14,:), /--/) 
title(’ACCELEROMETER BIAS’); xlabel(’time, minutes’); 
ylabel('acc. bias, meters/second“2'); grid 

subplot (223), 
plot(divemin,divex(15,1:n),'-',divemin,divexhat(15,:),'--') 
title(’ACCELEROMETER BIAS’); xlabel(’time, minutes’); 
ylabel(’acc. bias, meters/second*2’); grid; meta met26; 


esterr.m: 

$ Steve Nagengast $ plots error estimate errors 
after dives 

% filename: esterr.m 


clear; clg 
load sn2 
ldel sn?.met 


divex = divex(:,l:numbdives); 

ester = divex - divexhat; 

subplot(221), plot(ester(1,:)); title(’ ATT. ERROR EST. 
ERROR’); 

xlabel(’dive number’); ylabel(’radians per second’); grid 
subplot(222), plot(ester(2,:)); title(” ATT. ERROR EST. 
ERROR’ ) ; 

xlabel(’dive number’); ylabel(’radians per second’); grid 
subplot(223), plot(ester(3,:)); title(’ ATT. ERROR EST. 
ERROR’); 


xlabel(’dive number’); ylabel(’radians per second’); grid; 
meta snl; clg 


subplot(221), plot(ester(4,:)); title(“POSITION ERROR 
ESTIMATE ERROR’); 

xlabel(’dive number’); ylabel(’meters’); grid 

subplot(222), plot(ester(5,:)); title(’POSITION ERROR 
ESTIMATE ERROR’) ; 

xlabel(’dive number’); ylabel(’meters’); grid 

subplot(223), plot(ester(6,:)); title(’POSITION ERROR 
ESTIMATE ERROR’) ; 

xlabel(’dive number’); ylabel(’meters’); grid; meta sn2; clg 
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subplot(221), plot(ester(7,:)); title('VELOCITY ERROR 
ESTIMATE ERROR'); 

xlabel('dive number'/); ylabel(’meters per second’); grid 
subplot(222), plot(ester(8,:)); title('VELOCITY ERROR 
ESTIMATE ERROR'); 

Xlabel(’dive number’); ylabel(’meters per second’); grid 
subplot(223), plot(ester(9,:)); title(’VELOCITY ERROR 
ESTIMATE ERROR’); 

xlabel('dive number'/); ylabel('meters per second'); grid; 
meta sn3; clg 


subplot(221), plot(ester(10,:)); title(’ GYRO DRIFT EST. 
ERROR’); 

xlabel(’dive number’); ylabel(’radians per second’); grid 
subplot(222), plot(ester(11,:)); title(' GYRO DRIFT EST. 
ERROR’); 

Xxlabel(’dive number’); ylabel(’radians per second’); grid 
subplot(223), plot(ester(12,:)); title(' GYRO DRIFT EST. 
ERROR’) ; 


Xlabel(’dive number’); ylabel(’radians per second’); grid; 
meta sn4; clg 


GubplIoE (221), plot(ester(13,:)); title(' ACC. BIAS EST. 
ERROR’); 

Xlabel(’dive number’); ylabel(’meters per second squared’); 
grid 

Euŭbplot(222), plot(ester(14,:)); title(' ACC. BIAS EST. 
ERROR’); 

xlabel('dive number’); ylabel(’meters per second squared’); 
grid 


Subplot(223), plot(ester(15,:)); title(’ ACC. BIAS EST. 
ERROR’); 

xlabel(’dive number’); ylabel(’meters per second squared’) ; 
grid; 


meta sn5; 
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